import json
import logging
import math
import os
import random
import sys
import uuid
from collections import Counter
from datetime import datetime
from math import sin, asin, cos, radians, sqrt, degrees, atan2

import sumolib
import traci
import traci.constants as tc
from traci import TraCIException

import platoonUtils
import scene
from httpserver import HttpInterface
from httpserver.json_response import JsonResponse
import sumo_main

# 输出异常信息
errorMsg = None
# 保存每次增加车辆的车辆信息
carIAddInfo = {}
# 车辆添加后，车辆不一定在IdList中出现，需要设置变量保存已经在idList中出现的变量
carInIdList = []
oneCarId = None
# 指定速度发车临时变量
carSpeedTemp = {}
carIdTemp = []
plexe = None
population = [15, 3, 1]
car_type_dict = {
    "carWeight": 1,
    "busWeight": 3,
    "truckWeight": 15
}
weights = [(1, 0.7), (3, 0.2), (15, 0.1)]
carMap = {}
AreaList = []
msgTemp = {}

emergency_traffic_list = list()
exclusive_traffic_list = list()
LANE_SPLICER = "_"
JUN_SPLICER = ":"
INTERNAL_SIGN = "internal"

# 全局发车id
vehicle_id = 0


def add_vehicle(depart: dict, sumo_net):
    """
        添加发车信息
    """
    global vehicle_id
    assert 'point' in depart.keys(), '未配置发车点'
    assert 'count' in depart.keys(), '发车数量为空'

    # print("发车信息为",depart)

    # 获取发车点位置
    start_edge_id, start_pos, start_lane_index = traci.simulation.convertRoad(depart['point'][0], depart['point'][1],
                                                                              True)
    single_edge = sumo_net.getEdge(start_edge_id)
    car_num = depart['count']
    # 获取各个车型比例
    weight_list = []
    for key, value in depart.items():
        if key in car_type_dict:
            car_type = car_type_dict.get(key)
            if value is not None and car_num != 0:
                weight_list.append((car_type, value / car_num))
    if single_edge is not None:
        temp = [start_edge_id]
        count = 0
        while single_edge is not None:
            edge_bound = single_edge.getOutgoing()
            edge_ids = list()
            for k, v in edge_bound.items():
                if '-361.0.00' in k.getID() or '-254.0.00' in k.getID() or '371.0.00' in k.getID():
                    continue
                edge_ids.append(k.getID())
            if edge_ids is not None and len(edge_ids) > 0:
                try:
                    id = random.choice(edge_ids)
                    temp.append(id)
                    single_edge = sumo_net.getEdge(id)
                except ValueError:
                    continue
            count += 1
            if len(temp) > 1000 or count > 200 or '-547.0.00' in temp:
                print(temp)
                break
        route_id = str(uuid.uuid1())
        print(temp)
        traci.route.add(route_id, temp)
        for i in range(car_num):
            element = random.choices(population=list(car_type_dict.values()), weights=[w[1] for w in weights], k=1)[0]
            if vehicle_id < 65330:
                vehicle_id = vehicle_id + 1
            else:
                vehicle_id = 0
            vehID = str(vehicle_id)
            length = traci.lane.getLength(start_edge_id + "_" + str(start_lane_index))
            # 车辆初始位置，随机生成[0, 200]之间的值
            position = random.uniform(0, length)
            # 车辆初始速度，随机生成[5, 15]之间的值
            speed = random.uniform(5, 15)

            traci.vehicle.add(vehID=vehID, routeID=route_id, typeID=str(element), depart='0',
                              departLane='free', departPos=str(position), departSpeed=str(speed))
    else:
        print("Road marking problem1")
        errorMsg = "标注错误，请重新标注"


def MTF_add_vehicle(routeId,depart: dict):
    """
        添加发车信息
    """
    global vehicle_id
    assert 'count' in depart.keys(), '发车数量为空'


    # print("发车信息为",depart)

    # 获取发车点位置
    # start_edge_id, start_pos, start_lane_index = traci.simulation.convertRoad(depart['point'][0], depart['point'][1],
    #                                                                           True)
    # # print(start_edge_id)
    # route_id = -1
    # if start_edge_id == "-326" or start_edge_id == "-276":
    #     route_id = "route_down"
    # elif start_edge_id == "-277.14" or start_edge_id == "-325":
    #     route_id = "route_up"
    # else:
    #     raise "发车位置错误"
    route_id = routeId
    route_edges = traci.route.getEdges(route_id)
    car_num = depart['count']
    # 获取各个车型比例
    weight_list = []
    for key, value in depart.items():
        if key in car_type_dict:
            car_type = car_type_dict.get(key)
            if value is not None and car_num != 0:
                weight_list.append((car_type, value / 100))
            if value is None and car_num != 0:
                weight_list.append((car_type, 0))

    time_count = 0
    simulation_time = traci.simulation.getTime()
    for i in range(car_num):
        element = random.choices(population=[w[0] for w in weight_list], weights=[w[1] for w in weight_list], k=1)[0]

        if vehicle_id < 65330:
            vehicle_id = vehicle_id + 1
        else:
            vehicle_id = 0
        vehID = str(vehicle_id)
        length = traci.lane.getLength(route_edges[0] + "_" + "0")
        # 车辆初始位置，随机生成[0, 200]之间的值
        position = random.uniform(0, length)
        # 车辆初始速度，随机生成[5, 15]之间的值
        speed = random.uniform(5, 15)
        traci.vehicle.add(vehID=vehID, routeID=route_id, typeID=str(element), depart=str(int(simulation_time+time_count)),
                          departLane='free', departPos=str(position), departSpeed=str(speed))
        sumo_main.depart_info[vehID] = {"routeID": route_id, "typeID": str(element), "departPos": str(position),
                                          "departSpeed": str(speed)}
        time_count +=3


def MTF_add_vehicle_disappear(arrived_vehicles_ids,temp):
    for vehicle_id in arrived_vehicles_ids:
        if vehicle_id in sumo_main.depart_info.keys():
            depart_vehicle_info = sumo_main.depart_info[vehicle_id]
            car_id = str(int(vehicle_id) + 1)
            while car_id in sumo_main.depart_info.keys():
                car_id = str(int(car_id) + 1)

            traci.vehicle.add(vehID=car_id, routeID=depart_vehicle_info["routeID"],
                              typeID=depart_vehicle_info["typeID"], depart='0',
                              departLane='free', departPos=depart_vehicle_info["departPos"],
                              departSpeed=depart_vehicle_info["departSpeed"])
            sumo_main.depart_info[car_id] = depart_vehicle_info
            


def addVehicle(vehicleNum, vehiclePoints, sumo_net):
    vehiclePointsobj = json.loads(vehiclePoints)
    vid_count = 0
    for vehiclePoint in vehiclePointsobj:
        lat = float(vehiclePoint["lat"])
        lon = float(vehiclePoint["lon"])
        startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(lon, lat, True)
        single_edge = sumo_net.getEdge(startedgeID)
        if single_edge is not None:
            temp = [startedgeID]
            count = 0
            while single_edge is not None:
                edge_Bound = single_edge.getOutgoing()
                edgeids = list()
                for k, v in edge_Bound.items():
                    edgeids.append(k.getID())
                if edgeids is not None and len(edgeids) > 0:
                    try:
                        id = random.choice(edgeids)
                        if True:
                            temp.append(id)
                        else:
                            logging.info("Road marking problem1")
                            errorMsg = "标注错误，请重新标注"
                        single_edge = sumo_net.getEdge(id)
                    except ValueError:
                        continue
                    # break
                count += 1
                if len(temp) > 1000 or count > 200 or '-547.0.00' in temp:
                    print(temp)
                    break
            routeid = str(uuid.uuid1())

            traci.route.add(routeid, temp)

            for i in range(vehicleNum):
                element = random.choices(population=list(population), weights=[w[1] for w in weights], k=1)[0]
                vehID = str(uuid.uuid4())
                # print(vehID)
                # speed = traci.vehicletype.getMaxSpeed(str(element))
                # print("speed: ",speed)
                # if vid_count == 0:
                #     # platoonUtils.add_vehicle1(plexe, vehID, startpos, startlaneIndex, 20, "passenger2")
                #     traci.vehicle.add(vehID=vehID, routeID=routeid, typeID=str(element), depart='0',
                #                       departLane=startlaneIndex, departPos=startpos, departSpeed=15)
                #     vid_count += 1
                # else:
                length = traci.lane.getLength(startedgeID + "_" + str(startlaneIndex))
                position = random.uniform(0, length)  # 车辆初始位置，随机生成[0, 200]之间的值
                speed = random.uniform(5, 15)  # 车辆初始速度，随机生成[5, 15]之间的值
                # print(1)
                # platoonUtils.add_vehicle1(plexe, vehID, startpos, startlaneIndex, speed, "passenger2")
                traci.vehicle.add(vehID=vehID, routeID=routeid, typeID=str(element), depart='0',
                                  departLane=startlaneIndex, departPos=str(position), departSpeed=str(speed))
                # traci.vehicletype.setAccel(str(element),0.7)



        else:
            logging.info("Road marking problem1")
            errorMsg = "标注错误，请重新标注"


# 事故区辅助变道车辆
auxiliary_list = {}

in_accident_area_veh_list = []
pelxe_falge = 0


def add_accident_area(accident: dict):
    """
        通过json 控制添加启动事故区
    """
    accident_list = accident['areaPoints']
    """
        添加事故区
        :return:
        """
    edge_line = list()
    accident_pos = list()
    try:
        for point in accident_list:
            edge_id, _, lane_index = traci.simulation.convertRoad(point[0], point[1], True)
            edge_line.append(edge_id + '_' + str(lane_index))
            accident_pos.append(traci.simulation.convertGeo(point[1], point[0], True))
        most_edge = Counter(edge_line).most_common(1)
        if most_edge:
            edge = str(list(most_edge[0])[0]).split("_")[:-1][0]
            lane_index = str(list(most_edge[0])[0]).split("_")[-1:][0]
            scene.accident.set_new_accident(edge, int(lane_index), accident_pos, 1)
    except Exception as e:
        print("添加事故区失败", accident_list, e)


def add_accident():
    """
    添加事故区
    :return:
    """
    temp_list = HttpInterface.accident
    HttpInterface.accident = list()
    edge_line = list()
    accident_pos = list()
    try:
        for point in temp_list:
            edge_id, _, lane_index = traci.simulation.convertRoad(point[1], point[0], True)
            edge_line.append(edge_id + '_' + str(lane_index))
            accident_pos.append(traci.simulation.convertGeo(point[1], point[0], True))
        most_edge = Counter(edge_line).most_common(1)
        if most_edge:
            edge = str(list(most_edge[0])[0]).split("_")[:-1][0]
            lane_index = str(list(most_edge[0])[0]).split("_")[-1:][0]
            scene.accident.set_new_accident(edge, int(lane_index), accident_pos, 1)
    except Exception as e:
        HttpInterface.accidentResponse = JsonResponse.error(str(e))
    else:
        HttpInterface.accidentResponse = JsonResponse.success_with_msg('添加成功')


def control_accident():
    """
    控制事故区
    :return:
    """
    try:
        aa = scene.accident.get_accident_polygon_list()
        control_param = HttpInterface.accidentControl
        HttpInterface.accidentControl = None
        accident_id = control_param['accidentId']
        switch = control_param['switch']
        scene.accident.control_accident(accident_id, switch)
    except Exception as e:
        HttpInterface.accidentResponse = JsonResponse.error(str(e))
    else:
        HttpInterface.accidentResponse = JsonResponse.success_with_msg('设置成功')


def simulation_control_accident():
    """
    控制事故区
    :return:
    """
    try:
        control_param = HttpInterface.accidentControl
        HttpInterface.accidentControl = None
        accident_id = control_param['accidentId']
        switch = control_param['switch']
        scene.accident.simulation_control_accident(accident_id, switch)
    except Exception as e:
        HttpInterface.accidentResponse = JsonResponse.error(str(e))
    else:
        HttpInterface.accidentResponse = JsonResponse.success_with_msg('设置成功')


def changeLane(plexe, vehicles: dict, polygon: dict):
    """
    变道(dict, dict) -> None
    :param vehicles: 事故区监控到的车辆信息
    :param polygon: 事故区信息
    """
    # print("===================== change lane =====================")
    global in_accident_area_veh_list, pelxe_falge
    for veh_id, veh_info in vehicles.items():
        if veh_info[tc.VAR_TYPE] == '0' and veh_id not in in_accident_area_veh_list:
            # 获取目标车辆所在路段ID
            # print("-----------------------", veh_id)
            try:
                target_vehicle_lane_id = traci.vehicle.getLaneID(veh_id)
                # traci.vehicle.setLaneChangeMode(veh_id, 512)
                # 获取目标车辆所在路段对象
                # print(target_vehicle_lane_id)
                if len(target_vehicle_lane_id) > 0:
                    target_vehicle_lane = traci.lane.getEdgeID(target_vehicle_lane_id)
                    laneNumber = traci.edge.getLaneNumber(target_vehicle_lane)
                    # 获取目标车辆所在路段相邻车道上的所有车辆
                    vehicles_on_adjacent_lanes = []
                    for lane_index in range(laneNumber):
                        vehicles_on_adjacent_lanes += traci.lane.getLastStepVehicleIDs(
                            target_vehicle_lane + "_" + str(lane_index))
                    for vehicles_on_adjacent_lane in vehicles_on_adjacent_lanes:
                        if pelxe_falge == 0:
                            # traci.vehicle.changeLaneRelative(vehicles_on_adjacent_lane, -1, 5)
                            if len(vehicles_on_adjacent_lanes) < 4:
                                print('sf')
                                # traci.vehicle.changeLaneRelative('v.0.3', -1, 5)
                pelxe_falge += 1
                # print("编队进入事故区监控范围")
                # todo 头车进入范围后加入no_change_veh_dict且换道
                in_accident_area_veh_list.append(veh_id)
                if 'v.0.0' not in in_accident_area_veh_list and len(in_accident_area_veh_list) > 0:
                    traci.vehicle.changeLaneRelative(veh_id, 1, 5)
                    traci.vehicle.changeLaneRelative('v.0.0', 1, 5)
                    signals = traci.vehicle.getSignals(veh_id)
                    # print('signals: ', signals)
            except TraCIException as e:
                print(e)
                print("error+++++++++++++++++++++++", veh_id)


def in_accident_area(plexe):
    """
    进入事故区
    :param plexe:
    :return:
    """
    accident_veh_list = list()
    polygon_list = scene.accident.get_accident_polygon_list()
    if len(polygon_list) > 0:
        for item in polygon_list:
            result = traci.edge.getContextSubscriptionResults(item['edge'])
            if len(result) > 0:
                for veh_id in result.keys():
                    if veh_id not in accident_veh_list:
                        accident_veh_list.append(veh_id)
                changeLane(plexe, result, item)
    return accident_veh_list


def out_accident_area(vehicles: list, vehicle_ids):
    global pelxe_falge, in_accident_area_veh_list
    """
    车辆离开事故区域
    :param vehicles:
    :return:
    """
    if 'v.0.0' not in vehicles and 'v.0.0' in vehicle_ids:
        pelxe_falge = 0
        for in_accident_area_veh in in_accident_area_veh_list:
            # traci.vehicle.changeLaneRelative(in_accident_area_veh, 1, 5)
            traci.vehicle.setSpeed(in_accident_area_veh, -1)
        in_accident_area_veh_list = []


def add_bad_weather():
    print('')


# 车队编排
def platoonLaneChange(vehicleid, plexe1, msgArea):
    global AreaList, plexe
    if (msgArea is not None and 'Accident' in msgArea) or len(AreaList) > 0:
        if msgArea is not None and 'Accident' in msgArea:
            AccidentArea = msgArea.split("=")[1]
            AccidentArea = json.loads(AccidentArea)
            AreaList = AccidentArea['points']
    # 车队事故区变道
    if 'p.0' in vehicleid and 'p.1' in vehicleid and 'p.2' in vehicleid and 'p.3' in vehicleid and len(
            AreaList) > 0:
        lon1 = AreaList[0]["lon"]
        #
        lat1 = AreaList[0]["lat"]
        #
        lon3 = AreaList[3]["lon"]
        lat3 = AreaList[3]["lat"]
        # 施工区距离
        sg_dist = calculate_distance(lon1, lat1, lon3, lat3)
        # 施工区的点所在车道
        startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(lon1, lat1, True)
        # 车头
        headstock_pos = traci.vehicle.getPosition("p.0")
        headstock_lon, headstock_lat = traci.simulation.convertGeo(headstock_pos[0], headstock_pos[1])
        # 车尾
        tailstock_pos = traci.vehicle.getPosition("p.3")
        # 转换为经纬度
        tailstock_lon, tailstock_lat = traci.simulation.convertGeo(tailstock_pos[0], tailstock_pos[1])
        # 施工区(361.90,284.87)
        # construction_pos = traci.vehicle.getPosition("v0")
        # construction_pos = (356.35, 295.74)
        # construction_pos = (428.03, 325.41)
        # 转换为经纬度
        # lon1, lat1 = traci.simulation.convertGeo(construction_pos[0], construction_pos[1])

        tc_dist = calculate_distance(tailstock_lat, tailstock_lon, lat1, lon1)
        hc_dist = calculate_distance(headstock_lat, headstock_lon, lat1, lon1)
        # ht_dist = calculate_distance(headstock_lat, headstock_lon, tailstock_lat, tailstock_lon)
        # print(tc_dist)

        # 判断车道数
        edge_id = traci.vehicle.getRoadID("p.0")
        edge_id3 = traci.vehicle.getRoadID("p.3")
        vehicleIDs = traci.edge.getLastStepVehicleIDs(edge_id)
        num_lanes = traci.edge.getLaneNumber(edge_id)

        if tc_dist > sg_dist and hc_dist > tc_dist and num_lanes > 3:
            plexe1.enable_auto_lane_changing(platoonUtils.LEADER, True)

            for i in range(platoonUtils.N_VEHICLES):
                edge_id = traci.vehicle.getRoadID("p." + str(i))
                num_lanes = traci.edge.getLaneNumber(edge_id)
                if num_lanes > 4:
                    traci.vehicle.changeLane("p." + str(i), 0, 10)
                elif num_lanes == 4:
                    traci.vehicle.changeLane("p." + str(i), num_lanes - 1, 10)
                elif num_lanes == 3:
                    traci.vehicle.changeLane("p." + str(i), num_lanes - 2, 10)
        elif hc_dist < 20 and hc_dist < tc_dist and num_lanes > 2:
            for i in range(platoonUtils.N_VEHICLES):
                traci.vehicle.changeLane("p." + str(i), num_lanes - 2, 10)
        # elif "-301.0.00" in edge_id or ("-284.0.00" in edge_id and HttpInterface.emergencyLaneSwitch == 1):
        #     for i in range(platoonUtils.N_VEHICLES):
        #         traci.vehicle.changeLane("p." + str(i), 0, 10)
        elif "-280.0.00" in edge_id and HttpInterface.emergencyLaneSwitch == 0 and len(vehicleIDs) < 4:
            for i in range(platoonUtils.N_VEHICLES):
                traci.vehicle.changeLane("p." + str(i), 1, 10)
        elif "-320.0.00" in edge_id:
            for i in range(platoonUtils.N_VEHICLES):
                traci.vehicle.changeLane("p." + str(i), 3, 10)
                # traci.vehicle.changeLane("p.0", 0, 10)
                # traci.vehicle.changeLane("p.1", 0, 10)
                # traci.vehicle.changeLane("p.2", 0, 10)
        elif "-547.0.00" in edge_id:
            traci.vehicle.remove("p.0")
            traci.vehicle.remove("p.1")
            traci.vehicle.remove("p.2")
            traci.vehicle.remove("p.3")
            plexe = None


# 判断两个点经纬度距离单位 m
def calculate_distance(jing1, wei1, jing2, wei2):
    lng1, lat1, lng2, lat2 = map(radians, [jing1, wei1, jing2, wei2])  # 经纬度转换成弧度
    dlon = lng2 - lng1
    dlat = lat2 - lat1
    a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2
    distance = 2 * asin(sqrt(a)) * 6371.393 * 1000  # 地球平均半径，6371.393km
    return distance


# 添加编道车队
def addplatoon():
    # global plexe
    # plexe = Plexe()
    # traci.addStepListener(plexe)
    platoonUtils.add_vehicles(plexe, platoonUtils.N_VEHICLES, 90, real_engine=False)
    # platoonUtils.add_vehicle1(plexe, "v0", 100, 0, 20, "1")
    # add_vehicle(plexe, "v1", 250, 0, 20, "passenger2")
    # traci.gui.trackVehicle("View #0", platoonUtils.LEADER)
    # traci.gui.setZoom("View #0", 1000)


# 前方超车算法
def ChangingLanesAheadToOvertake(vehicles):
    for vehicle_id in vehicles:
        # 获取当前车辆速度
        speed = traci.vehicle.getSpeed(vehicle_id)

        # 获取当前车辆前面的车辆ID
        leader_id = traci.vehicle.getLeader(vehicle_id)

        # 如果当前车辆有前车
        if leader_id is not None:
            # 获取当前车辆与前车之间的距离
            # vehicle_id_pos=traci.vehicle.getPosition(vehicle_id)
            # leader_id_pos=traci.vehicle.getPosition(leader_id)
            # longitude1, latitude1 = traci.simulation.convertGeo(vehicle_id_pos[0], vehicle_id_pos[1])
            # longitude2, latitude2 = traci.simulation.convertGeo(leader_id_pos[0], leader_id_pos[1])
            # distance=calculate_distance(longitude1,latitude1,longitude2,latitude2)

            distance = leader_id[1]

            # 获取前车速度
            leader_speed = traci.vehicle.getSpeed(leader_id[0])

            # 如果当前车辆速度小于前车速度并且距离小于10米
            if speed > leader_speed and 50 > distance > 15:
                pos = traci.vehicle.getPosition(vehicle_id)
                edgeID, pos, lanindex = traci.simulation.convertRoad(pos[0], pos[1])
                num_lanes = traci.edge.getLaneNumber(edgeID)
                # 尝试向左变道
                # if num_lanes>1:
                #     if lanindex == 0:
                #         traci.vehicle.changeLane(vehicle_id, 1, 10)
                #     elif lanindex == 1:
                #         traci.vehicle.changeLane(vehicle_id, 0, 10)
                #     elif lanindex == 2:
                #         traci.vehicle.changeLane(vehicle_id, 1, 10)
                #     elif lanindex == 3:
                #         traci.vehicle.changeLane(vehicle_id, 2, 10)


# 超车
def Overtaking(tag_vehicle_id, global_vehicles):
    try:
        # 获取当前车辆的位置和速度信息
        # vehicle_pos = traci.vehicle.getPosition(vehicle_id)
        # vehicle_speed = traci.vehicle.getSpeed(vehicle_id)
        # 判断主车前方是否有车
        if "v.0.0" in tag_vehicle_id:
            leader_ids = ['v.0.0', 'v.0.1', 'v.0.2', 'v.0.3']
            surrounding_vehicles = get_surrounding_vehicles(tag_vehicle_id, global_vehicles, 50)
            signals = traci.vehicle.getSignals('v.0.0')
            # print('signals: ', signals)
            for surrounding_vehicle in surrounding_vehicles:
                traci.vehicle.setSignals(surrounding_vehicle, signals)

                if surrounding_vehicle not in leader_ids and ('-284.0.00' in traci.vehicle.getRoadID(
                        surrounding_vehicle) or '-285.0.00' in traci.vehicle.getRoadID(surrounding_vehicle)):
                    traci.vehicle.setSignals(surrounding_vehicle, 8)
                    traci.vehicle.setLaneChangeMode(surrounding_vehicle, 512)
                    # continue
                if surrounding_vehicle in leader_ids and ('-284.0.00' in traci.vehicle.getRoadID(
                        surrounding_vehicle) or '-285.0.00' in traci.vehicle.getRoadID(surrounding_vehicle)):
                    traci.vehicle.setSignals(surrounding_vehicle, 2)
                    traci.vehicle.setLaneChangeMode(surrounding_vehicle, 0)

            # print('surrounding_vehicles: ',surrounding_vehicles)
            # p_leader_id=traci.vehicle.subscribeLeader(vehicle_id)
            # 获取前车的位置和速度信息
            p_leader_id = traci.vehicle.getLeader(vehicle_id)
            # p_leader_id = veh_info[tc.VAR_LEADER]
            if p_leader_id is not None:
                if p_leader_id[1] < 100:
                    # traci.vehicle.setSpeed(vehicle_id,-1)
                    traci.vehicle.changeLaneRelative(p_leader_id[0], -1, 10)
        # # 获取前车的位置和速度信息
        # leader_id = traci.vehicle.getLeader(vehicle_id)
        # if leader_id is not None:
        #     leader_pos = traci.vehicle.getPosition(leader_id[0])
        #     leader_speed = traci.vehicle.getSpeed(leader_id[0])
        #
        #     # 计算两车之间的距离
        #     # distance = math.sqrt((leader_pos[0] - vehicle_pos[0]) ** 2 + (leader_pos[1] - vehicle_pos[1]) ** 2)
        #
        #     # 判断两车之间的距离是否大于150
        #     if leader_id[1] > 150:
        #         traci.vehicle.setSpeed(vehicle_id, leader_speed + 10)  # 加速行驶
        #         traci.vehicle.setAcceleration(vehicle_id, 2, 5)
        #     else:
        #         traci.vehicle.setSpeed(vehicle_id, vehicle_speed)  # 维持当前速度
    except Exception as e:
        logging.info("sumo Error" + str(e))
    #     # clearmessage()


# 判断是否触发场景区域
# 判断车辆到达距离限速点距离长度距离
distance1 = 0
# 创建5个点之间的距离
dists = []
accelerations = []

# a = {}
# a.keys()
# id in a.keys():
# a[id] = dis
distance_dicts = {}
speed_dicts = {}
speed_plexe = 0
leader_ids = ['v.0.0', 'v.0.1', 'v.0.2', 'v.0.3']


def is_point_in_area(vehicles: dict, speed_limit_points: list, speed_speeds: list):
    global distance1, dists, accelerations, distance_dicts, speed_plexe, leader_ids
    # 获取到场景区域内的车辆
    # 对场景区域内的车辆进行限速设置
    array = []
    try:
        for vehicle_id, vehicle_info in vehicles.items():
            # if vehicle_id in leader_ids:
            #     # traci.vehicle.setLaneChangeMode(vehicle_id, 512)
            #     # traci.vehicle.setMinGap(vehicle_id, 5)  # 定义与前车的最小间距
            #     continue
            array.append(vehicle_id)
            # 如果车辆此时没在这个区域内，则可能是刚刚进入这个区域，也可能已经处于这个区域内
            if vehicle_id not in distance_dicts:
                veh_distance = 99999999
                number = 0
                # 此时判断离该车辆最近的经纬度点
                for i in range(len(speed_limit_points)):
                    lat = float(speed_limit_points[i]["lat"])
                    lon = float(speed_limit_points[i]["lon"])
                    veh_pos = vehicle_info[tc.VAR_POSITION]
                    veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                    veh_distance2 = calculate_distance(veh_lon, veh_lat, lon, lat)
                    if veh_distance2 <= veh_distance:
                        number = i
                        veh_distance = veh_distance2
                        distance_dicts[vehicle_id] = [lat, lon, speed_speeds[i]]
                # 计算车辆的加速度
                acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                      float(speed_speeds[number]) / 3.6, veh_distance)
                t = calc_time_to_reach2(float(speed_speeds[number]) / 3.6, str(veh_distance),
                                        vehicle_info[tc.VAR_SPEED], acceleration)
                print("主路限速生效",vehicle_id,speed_speeds[number])
                traci.vehicle.slowDown(vehicle_id, float(speed_speeds[number]) / 3.6, t)
                # traci.vehicle.setSpeed(vehicle_id, float(HttpInterface.speeds[number])/3.6)
            else:
                # 首先判断车辆是否已经经过了这个点位（误差距离5m）
                points = distance_dicts[vehicle_id]
                veh_pos = vehicle_info[tc.VAR_POSITION]
                veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                distance = calculate_distance(veh_lon, veh_lat, points[1], points[0])
                # 通过判断车辆车速 对未生效的车辆速度进行校正
                if vehicle_info[tc.VAR_SPEED] > float(points[2]) / 3.6:
                    acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                          float(points[2]) / 3.6, distance)
                    t = calc_time_to_reach2(float(points[2]) / 3.6, str(distance),
                                            vehicle_info[tc.VAR_SPEED], acceleration)

                    traci.vehicle.slowDown(vehicle_id, float(points[2]) / 3.6, t)
                    # traci.vehicle.setSpeed(vehicle_id, float(points[2]) / 3.6)
                if distance < 10:
                    # 说明车辆已经经过了这个点位，则需要判断下一个点位
                    number = 0
                    for i in range(len(speed_limit_points)):
                        lat = float(speed_limit_points[i]["lat"])
                        lon = float(speed_limit_points[i]["lon"])
                        if lat == points[0] and lon == points[1]:
                            number = i
                            break
                    # print(vehicles.items())
                    if number == len(speed_limit_points) - 1:
                        continue
                    else:
                        # 说明车辆还没有经过最后一个点位，则需要判断下一个点位
                        lat = float(speed_limit_points[number + 1]["lat"])
                        lon = float(speed_limit_points[number + 1]["lon"])
                        distance_dicts[vehicle_id] = [lat, lon, speed_speeds[number + 1]]
                        veh_pos = vehicle_info[tc.VAR_POSITION]
                        veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                        distance = calculate_distance(veh_lon, veh_lat, lon, lat)
                        acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                              float(speed_speeds[number + 1]) / 3.6, distance)
                        t = calc_time_to_reach2(float(speed_speeds[number + 1]) / 3.6, str(distance),
                                                vehicle_info[tc.VAR_SPEED], acceleration)
                        print("主路限速生效", vehicle_id, speed_speeds[number])
                        traci.vehicle.slowDown(vehicle_id, float(speed_speeds[number + 1]) / 3.6, t)
                        # traci.vehicle.setSpeed(vehicle_id, float(HttpInterface.speeds[number+1]) / 3.6)
        # 获取数据在array里面，而不在distance_dicts的keys中的数据
        resultList = [a for a in distance_dicts.keys() if a not in array]
        for i in resultList:
            # todo 判断车辆是否在网
            distance_dicts.pop(i)
            traci.vehicle.setSpeed(i, -1)
    except Exception as e:
        print(e)


def is_point_in_polygon(vehicles: dict, plexe):
    global distance1, dists, accelerations, distance_dicts, speed_plexe
    # print(vehicles)

    # 对场景区域内的车辆进行限速设置
    for vehicle_id, vehicle_info in vehicles.items():
        if vehicle_id not in distance_dicts:
            distance_dicts[vehicle_id] = vehicle_info[tc.VAR_DISTANCE]
            distance_dicts["speed"] = vehicle_info[tc.VAR_SPEED]
        if vehicle_id not in speed_dicts:
            speed_dicts[vehicle_id] = vehicle_info[tc.VAR_SPEED]
        # 初始化进入触发区速度，加速度，距离
        if distance1 == 0:
            # 0,1 0,2 0,3 0,4距离
            dist_init = 0
            for i in range(len(HttpInterface.speedLimitPoints) - 1):
                lat1 = float(HttpInterface.speedLimitPoints[i]["lat"])
                lon1 = float(HttpInterface.speedLimitPoints[i]["lon"])
                lat2 = float(HttpInterface.speedLimitPoints[i + 1]["lat"])
                lon2 = float(HttpInterface.speedLimitPoints[i + 1]["lon"])
                dist = calculate_distance(lon1, lat1, lon2, lat2)
                # 求加速度
                acceleration = calculate_acceleration(float(HttpInterface.speeds[i]) / 3.6,
                                                      float(HttpInterface.speeds[i + 1]) / 3.6, dist)
                accelerations.append(acceleration)
                dist_init = dist + dist_init
                dists.append(dist_init)
            veh_speed = vehicle_info[tc.VAR_SPEED]
            veh_pos = vehicle_info[tc.VAR_POSITION]
            veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
            veh_distance = calculate_distance(veh_lon, veh_lat, HttpInterface.speedLimitPoints[0]['lon'],
                                              HttpInterface.speedLimitPoints[0]['lat'])
            veh_acc = calculate_acceleration(veh_speed, float(HttpInterface.speeds[0]) / 3.6, veh_distance)
            HttpInterface.speeds.insert(0, veh_speed)
            accelerations.insert(0, veh_acc)
            dists.insert(0, veh_distance)
            for s in range(len(dists)):
                if s > 0:
                    dists[s] = dists[s] + dists[0]
            # 初始化限速盘第一个点与场景区域之间的距离
            distance1 = vehicle_info[tc.VAR_DISTANCE]
        # 初始化限速盘第一个点与场景区域之间的加速度
        init = calculate_acceleration(distance_dicts["speed"], float(HttpInterface.speeds[1]) / 3.6, distance1)
        accelerations[0] = init
        distance = vehicle_info[tc.VAR_DISTANCE] - distance_dicts[vehicle_id]

        isReachTheSpecifiedPoint(vehicle_id, dists, accelerations, vehicle_info[tc.VAR_TYPE],
                                 distance, plexe)
        #       判断是否为编队车辆
        if vehicle_id == 'v.0.0':
            if speed_plexe == 0:
                speed_plexe = vehicle_info[tc.VAR_SPEED]
                # 求加速度
                acceleration = calculate_acceleration(speed_plexe,
                                                      float(HttpInterface.speeds[1]) / 3.6, dists[0])
                accelerations[0] = acceleration
            distance = vehicle_info[tc.VAR_DISTANCE] - distance_dicts[vehicle_id]
            isPlexeReachTheSpecifiedPoint(dists, accelerations, distance, plexe)


# 已进入恶劣天气区域且已处理过的车辆
in_bw_area_veh_list = list()


def in_bw_area(vehicles: dict, polygon: dict):
    """
    车辆进入恶劣天气区域范围后
    :param vehicles:
    :param polygon:
    :return:
    """
    global in_bw_area_veh_list
    for vehicle_id, vehicle_info in vehicles.items():
        speed = vehicle_info[tc.VAR_SPEED]
        speed_limit = polygon['speedLimit'] / 3.6
        if speed > speed_limit:
            if vehicle_info[tc.VAR_TYPE] != '0':
                if vehicle_id not in in_bw_area_veh_list:
                    in_bw_area_veh_list.append(vehicle_id)
                    accel = (speed_limit ** 2 - speed ** 2) / (2 * 35)
                    time = (speed_limit - speed) / accel
                    # print('veh_id:%s limit %s accel: %s time:%s' % (vehicle_id, speed_limit, accel, time))
                    traci.vehicle.setSpeed(vehicle_id, speed_limit)
            else:
                if 'v.0.0' not in in_bw_area_veh_list and 'v.0.3' not in in_bw_area_veh_list:
                    accel = (speed_limit ** 2 - speed ** 2) / (2 * 35)
                    time = (speed_limit - speed) / accel
                    # print('veh_id:%s limit %s accel: %s time:%s' % (vehicle_id, speed_limit, accel, time))
                    traci.vehicle.setAcceleration('v.0.0', accel - 0.2, time * 2)
                    # traci.vehicle.slowDown('v.0.0', speed_limit, time)
                    # plexe.set_cc_desired_speed('v.0.0', speed_limit)
                    # plexe.set_fixed_acceleration('v.0.0', True, accel)
                if vehicle_id not in in_bw_area_veh_list:
                    in_bw_area_veh_list.append(vehicle_id)


def MTF_in_bw_area(vehicles: dict, polygon: dict):
    """
    车辆进入恶劣天气区域范围后
    :param vehicles:
    :param polygon:
    :return:
    """
    global in_bw_area_veh_list

    for vehicle_id, vehicle_info in vehicles.items():
        speed = vehicle_info[tc.VAR_SPEED]
        speed_limit = polygon['speedLimit'] / 3.6
        if speed > speed_limit:
            if vehicle_info[tc.VAR_TYPE] != 'CV' and vehicle_info[tc.VAR_TYPE] != 'AV' and vehicle_info[tc.VAR_TYPE] != 'HDV' :
                if vehicle_id not in in_bw_area_veh_list:
                    in_bw_area_veh_list.append(vehicle_id)

                # accel = (speed_limit ** 2 - speed ** 2) / (2 * 35)
                # time = (speed_limit - speed) / accel
                # print('veh_id:%s limit %s accel: %s time:%s' % (vehicle_id, speed_limit, accel, time))
                traci.vehicle.setSpeed(vehicle_id, speed_limit)
                # print(vehicle_id,"车速为",traci.vehicle.getSpeed(vehicle_id)  * 3.6,"进入天气环境")


















def out_bw_area(vehicles: list):
    """
    车辆离开恶劣天气区域
    :param vehicles:
    :return:
    """
    global in_bw_area_veh_list
    if len(in_bw_area_veh_list) > 0:
        for item in in_bw_area_veh_list[:]:
            if item not in vehicles:
                if 'v.0.' not in item:
                    try:
                        traci.vehicle.setSpeed(item, -1)
                    except TraCIException as t:
                        print("ERROR : 速度设置异常 -> ", str(t))
                in_bw_area_veh_list.remove(item)


def MTF_out_bw_area(vehicles: list):
    """
    车辆离开恶劣天气区域，将离开车辆的区域的速度交给sumo托管
    :param vehicles:实时更新的在恶劣天气半径内的车
    in_bw_area_veh_list：已经到达恶劣天气半径内，并且已经进行限速的车
    :return:
    """
    global in_bw_area_veh_list

    if len(in_bw_area_veh_list) > 0:

        for item in in_bw_area_veh_list[:]:
            if item not in vehicles:
                try:
                    # print(item,"！！！！！！！！！！！！！！！离开环境==========")
                    traci.vehicle.setSpeed(item, -1)
                except TraCIException as t:
                    print("ERROR : 速度设置异常 -> ", str(t))
                in_bw_area_veh_list.remove(item)



# 添加事故区
# 判断车辆是否进入施工区前默认为0，等1说明触发施工区
status = 0
vei_distance = 0


def addAccidentArea(msgArea, vehicle, plexe1):
    # msgArea='AccidentArea: ={"flage":"5","points":[{"lon":108.89547178978344,"lat":34.37424312531908},{"lon":108.89547962421929,"lat":34.374221509800755},{"lon":108.89654344539252,"lat":34.37448715515829},{"lon":108.89652839800766,"lat":34.37451773879037}]}'
    global AreaList, status, vei_distance
    if (msgArea is not None and 'Accident' in msgArea) or len(AreaList) > 0:
        if msgArea is not None and 'Accident' in msgArea:
            AccidentArea = msgArea.split("=")[1]
            AccidentArea = json.loads(AccidentArea)
            AreaList = AccidentArea['points']
        # vehicles=traci.vehicle.getIDList()
        lon1 = AreaList[0]["lon"]
        #
        lat1 = AreaList[0]["lat"]
        #
        lon3 = AreaList[3]["lon"]
        lat3 = AreaList[3]["lat"]
        # 施工区距离
        sg_dist = calculate_distance(lon1, lat1, lon3, lat3)
        # for vehicle in vehicles:
        if 'p.' not in vehicle:
            # 判断车道数
            edge_id = traci.vehicle.getRoadID(vehicle)

            num_lanes = traci.edge.getLaneNumber(edge_id)
            x, y = traci.vehicle.getPosition(vehicle)  # 当前车辆的位置坐标
            edgeID, pos, lanindex = traci.simulation.convertRoad(x, y)
            angle = traci.vehicle.getAngle(vehicle)  # 当前车辆的朝向角度
            dx = 10 * math.cos(math.radians(angle))
            dy = 10 * math.sin(math.radians(angle))
            lon, lat = traci.simulation.convertGeo(x, y)
            # 判断车辆与施工区结尾点和起始点的距离
            veh_sc = calculate_distance(lon, lat, lon3, lat3)
            veh_sh = calculate_distance(lon, lat, lon1, lat1)
            # areas=[(point["lat"], point["lon"]) for point in AreaList]
            # polygon = geo.Polygon(areas)
            # vei_distancess = traci.vehicle.getDistance(vehicle)
            # print("veh_sh:{} veh_sc:{} vehicle{}".format(veh_sh,veh_sc,vehicle))
            if veh_sh < 2:
                traci.vehicle.changeLane(vehicle, 2, 10)
            elif veh_sc < 5:
                traci.vehicle.changeLane(vehicle, 3, 10)
        elif plexe1 is not None:
            # 施工区距离
            # sg_dist = calculate_distance(lon1, lat1, lon3, lat3)
            # 施工区的点所在车道
            # startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(lon1, lat1, True)
            try:
                # 车头
                headstock_pos = traci.vehicle.getPosition("v.0.0")
                headstock_lon, headstock_lat = traci.simulation.convertGeo(headstock_pos[0], headstock_pos[1])
                # 车尾
                tailstock_pos = traci.vehicle.getPosition("p.3")
                # 转换为经纬度
                tailstock_lon, tailstock_lat = traci.simulation.convertGeo(tailstock_pos[0], tailstock_pos[1])
                # 施工区(361.90,284.87)
                # construction_pos = traci.vehicle.getPosition("v0")
                # construction_pos = (356.35, 295.74)
                # construction_pos = (428.03, 325.41)
                # 转换为经纬度
                # lon1, lat1 = traci.simulation.convertGeo(construction_pos[0], construction_pos[1])

                tc_dist = calculate_distance(tailstock_lat, tailstock_lon, lat1, lon1)
                hc_dist = calculate_distance(headstock_lat, headstock_lon, lat1, lon1)
                # ht_dist = calculate_distance(headstock_lat, headstock_lon, tailstock_lat, tailstock_lon)
                # print(tc_dist)

                # 判断车道数
                edge_id = traci.vehicle.getRoadID("v.0.0")
                # edge_id3 = traci.vehicle.getRoadID("p.3")
                vehicleIDs = traci.edge.getLastStepVehicleIDs(edge_id)
                num_lanes = traci.edge.getLaneNumber(edge_id)

                if tc_dist > sg_dist and hc_dist > tc_dist and num_lanes > 3:
                    plexe1.enable_auto_lane_changing(platoonUtils.LEADER, False)

                    for i in range(platoonUtils.N_VEHICLES):
                        edge_id = traci.vehicle.getRoadID("p." + str(i))
                        num_lanes = traci.edge.getLaneNumber(edge_id)
                        if num_lanes > 4:
                            traci.vehicle.changeLane("p." + str(i), 0, 10)
                        elif num_lanes == 4:
                            traci.vehicle.changeLane("p." + str(i), num_lanes - 1, 10)
                        elif num_lanes == 3:
                            traci.vehicle.changeLane("p." + str(i), num_lanes - 2, 10)
                elif hc_dist < 20 and hc_dist < tc_dist and num_lanes > 2:
                    for i in range(platoonUtils.N_VEHICLES):
                        traci.vehicle.changeLane("p." + str(i), num_lanes - 2, 10)
                # elif "-301.0.00" in edge_id or ("-284.0.00" in edge_id and HttpInterface.emergencyLaneSwitch == 1):
                #     for i in range(platoonUtils.N_VEHICLES):
                #         traci.vehicle.changeLane("p." + str(i), 0, 10)
                elif "-280.0.00" in edge_id and HttpInterface.emergencyLaneSwitch == 0 and len(vehicleIDs) < 4:
                    for i in range(platoonUtils.N_VEHICLES):
                        traci.vehicle.changeLane("p." + str(i), 1, 10)
                elif "-285.0.00" in edge_id:
                    for i in range(platoonUtils.N_VEHICLES):
                        traci.vehicle.changeLane("p." + str(i), 2, 10)
                elif "-301.0.00" in edge_id:
                    for i in range(platoonUtils.N_VEHICLES):
                        traci.vehicle.changeLane("p." + str(i), 0, 10)
                elif "-284.0.00" in edge_id:
                    for i in range(platoonUtils.N_VEHICLES):
                        traci.vehicle.changeLane("p." + str(i), 2, 10)
                        # traci.vehicle.changeLane("p.0", 0, 10)
                        # traci.vehicle.changeLane("p.1", 0, 10)
                        # traci.vehicle.changeLane("p.2", 0, 10)
                elif "-547.0.00" in edge_id:
                    traci.vehicle.remove("p.0")
                    traci.vehicle.remove("p.1")
                    traci.vehicle.remove("p.2")
                    traci.vehicle.remove("p.3")
                    plexe = None
            except TraCIException as e:
                logging.info("TraCIException", str(e))
            # elif status>0:
            #     traci.vehicle.changeLane(vehicle, 0, 10)
            #     status=0

    return None


# 限速区域
# 计算加速度减速度
# initial_velocity:初速度
# final_velocity:末速度
# displacement:位移
def calculate_acceleration(initial_velocity, final_velocity, displacement):
    acceleration = (final_velocity ** 2 - initial_velocity ** 2) / (2 * displacement)
    return acceleration


# 判断车辆是否到达指定点
def isReachTheSpecifiedPoint(vehicleid, dists, accelerations, typeid, distance, plexe):
    now = datetime.now()
    for i in range(len(dists) - 1):

        if distance <= dists[i] and i == 0:
            # plexe.set_fixed_acceleration('v.0.0', True, accelerations[i])
            # 打印年、月、日、时、分、秒和毫秒
            if (dists[i] - distance) < 1 or distance == 0.0:
                print(
                    "vehicleid: {} i{} time: {} speed: {} accelerations: {} dist: {} distance: {}".format(vehicleid, i,
                                                                                                          now.strftime(
                                                                                                              '%H:%M:%S.%f')[
                                                                                                          :-3],
                                                                                                          traci.vehicle.getSpeed(
                                                                                                              vehicleid) * 3.6,
                                                                                                          accelerations[
                                                                                                              i],
                                                                                                          dists[i],
                                                                                                          distance))

            if accelerations[i] < 0:
                time = (float(HttpInterface.speeds[i + 1]) / 3.6 - traci.vehicle.getSpeed(vehicleid)) / accelerations[
                    i] - 0.08
                if time > 0:
                    traci.vehicle.slowDown(vehicleid, float(HttpInterface.speeds[i + 1]) / 3.6, time)
                    # traci.vehicletype.setDecel(typeid, abs(accelerations[i]))

                # traci.vehicle.setAccel(typeid,0)
            else:
                startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(
                    HttpInterface.speedLimitPoints[i + 1]['lon'], HttpInterface.speedLimitPoints[i + 1]['lat'], True)
                lanenums = traci.edge.getLaneNumber(startedgeID)
                for j in range(lanenums):
                    traci.lane.setMaxSpeed(startedgeID + '_' + str(j), float(HttpInterface.speeds[i + 1]) / 3.6)
                time = (float(HttpInterface.speeds[i + 1]) / 3.6 - traci.vehicle.getSpeed(vehicleid)) / accelerations[i]
                if time > 0:
                    # traci.vehicle.slowDown(vehicleid, float(HttpInterface.speeds[i + 1]) / 3.6, time)
                    traci.vehicle.setSpeed(vehicleid, float(HttpInterface.speeds[i + 1]) / 3.6)
                    # traci.vehicletype.setAccel(typeid,0)
                    # traci.vehicletype.setMaxSpeed(typeid, float(HttpInterface.speeds[i]) / 3.6)
        elif dists[i] <= distance and distance <= dists[i + 1]:
            if (distance - dists[i]) < 1 or (dists[i + 1] - distance) < 1:
                # plexe.set_fixed_acceleration('v.0.0', True, accelerations[i+1])
                print(
                    "vehicleid: {} i{} time: {} speed: {} accelerations: {} dist: {} distance: {}".format(vehicleid, i,
                                                                                                          now.strftime(
                                                                                                              '%H:%M:%S.%f')[
                                                                                                          :-3],
                                                                                                          traci.vehicle.getSpeed(
                                                                                                              vehicleid) * 3.6,
                                                                                                          accelerations[
                                                                                                              i],
                                                                                                          dists[i],
                                                                                                          distance))

            # 打印年、月、日、时、分、秒和毫秒
            # print(vehicleid,'dist1: ',now.strftime('%H:%M:%S.%f')[:-3])0.08是距离误差
            if accelerations[i + 1] < 0:
                time = (float(HttpInterface.speeds[i + 2]) / 3.6 - traci.vehicle.getSpeed(vehicleid)) / accelerations[
                    i + 1] - 0.08
                # print(time)
                if time > 0:
                    traci.vehicle.slowDown(vehicleid, float(HttpInterface.speeds[i + 2]) / 3.6, time)
                    traci.vehicletype.setDecel(typeid, abs(accelerations[i + 1]))
                # traci.vehicle.setAccel(typeid, 0)
            else:
                if dists[i + 1] - distance < 80:
                    acc = calculate_acceleration(traci.vehicle.getSpeed(vehicleid),
                                                 float(HttpInterface.speeds[i + 2]) / 3.6, dists[i + 1] - distance)
                    time = (float(HttpInterface.speeds[i + 2]) / 3.6 - traci.vehicle.getSpeed(vehicleid)) / acc + 0.08
                    if time > 0:
                        speed = (dists[i + 1] - distance) / time
                        # traci.vehicle.setSpeed(vehicleid,speed)
                        traci.vehicle.slowDown(vehicleid, float(HttpInterface.speeds[i + 2]) / 3.6, time)
                        # traci.vehicletype.setAccel(typeid,accelerations[i+1])
                else:
                    startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(
                        HttpInterface.speedLimitPoints[i]['lon'], HttpInterface.speedLimitPoints[i]['lat'],
                        True)
                    lanenums = traci.edge.getLaneNumber(startedgeID)
                    for j in range(lanenums):
                        traci.lane.setMaxSpeed(startedgeID + '_' + str(j), float(HttpInterface.speeds[i + 1]) / 3.6)
                    traci.vehicle.slowDown(vehicleid, float(HttpInterface.speeds[i + 1]) / 3.6, 1)
                    # traci.vehicletype.setMaxSpeed(typeid, float(HttpInterface.speeds[i+1])/3.6)


# 设置编队减速加速
plexe_flage = 0


def isPlexeReachTheSpecifiedPoint(dists, accelerations, distance, plexe):
    now = datetime.now()
    for i in range(len(dists) - 1):

        if distance <= dists[i] and i == 0:
            plexe.set_fixed_acceleration('v.0.0', True, accelerations[i])
            plexe.set_fixed_acceleration('v.0.1', True, accelerations[i])
            plexe.set_fixed_acceleration('v.0.2', True, accelerations[i])
            plexe.set_fixed_acceleration('v.0.3', True, accelerations[i])
        elif dists[i] <= distance and distance <= dists[i + 1]:
            # if (distance - dists[i]) < 1 or (dists[i + 1] - distance) < 1:
            plexe.set_fixed_acceleration('v.0.0', True, accelerations[i + 1])
            plexe.set_fixed_acceleration('v.0.1', True, accelerations[i + 1])
            plexe.set_fixed_acceleration('v.0.2', True, accelerations[i + 1])
            plexe.set_fixed_acceleration('v.0.3', True, accelerations[i + 1])
            traci.vehicle.setLaneChangeMode()


OPEN = 1
DOWN = 0
DELETE = -1
EMERGENCY_LANE_ALLOW_VCLASS = ['emergency', 'authority', 'army', "custom1"]
EXCLUSIVE_LANE_ALLOW_VCLASS = ['custom1']
STOP_EDGE = '-285.0.00'


# 简单寻路(单边)
def findEdgesAndTraffics(start_edge, end_edge, tl_ids, stop_edge=None):
    edge_list = list()
    if start_edge is None or end_edge is None:
        return edge_list
    edge_list.append(start_edge.getID())
    if start_edge.getID() == end_edge.getID():
        return edge_list
    traffic_list = list()
    temp_edge = start_edge
    count = 0
    while temp_edge.getID() != end_edge.getID() and count < 100:
        if count == 0 and temp_edge.getFromNode().getID() in tl_ids:
            traffic_list.append(temp_edge.getFromNode().getID())
        for k, v in temp_edge.getOutgoing().items():
            temp_edge = k
            if stop_edge is not None and temp_edge.getID() == stop_edge:
                break
            if temp_edge.getFromNode().getID() in tl_ids:
                traffic_list.append(temp_edge.getFromNode().getID())
            edge_list.append(k.getID())
        count += 1
    return edge_list, traffic_list


# 初始化应急车道
def initEmergencyLaneConfig(start_edge, end_edge, tl_ids):
    # 1.计算edge集合和信号灯集合
    edge_list, traffic_list = findEdgesAndTraffics(start_edge, end_edge, tl_ids, stop_edge=STOP_EDGE)

    # 2.校验并添加应急车道
    for edge_id in edge_list:
        # 2.1.不可处于连接点
        assert ':' not in edge_id, '交叉路口无法设置应急车道'
        # 2.2.车道数量>2
        lane_number = traci.edge.getLaneNumber(edge_id)
        assert lane_number > 2, '道路数量小于3时无法设置应急车道'
        # 2.3.最右侧车道宽度>=3m
        lane_id = str(edge_id) + '_0'
        lane_width = traci.lane.getWidth(lane_id)
        assert lane_width >= 3, '路宽小于3m时无法设置应急车道'
        # 2.4.最右侧车道长度>=30m
        # lane_length = traci.lane.getLength(lane_id)
        # assert lane_length >= 30, '应急车道长度小于30m时无法设置应急车道'
        # 2.5.只允许特殊车辆通行
        traci.lane.setAllowed(lane_id, EMERGENCY_LANE_ALLOW_VCLASS)
        # 2.6.更新emergencyLaneSwitch属性
        HttpInterface.emergencyLaneSwitch[edge_id] = DOWN

    # 3.增加应急车道指示灯
    for traffic_id in traffic_list:
        state = traci.trafficlight.getRedYellowGreenState(traffic_id)
        # 应急车道在最右侧 所以替换第一个灯色
        state = 'r' + state[1:]
        traci.trafficlight.setRedYellowGreenState(traffic_id, state)
    message = "应急车道添加成功"
    return message


def controlEmergencyLane(action: int, edge, tl_ids):
    # 匹配道路

    # if HttpInterface.emergencyLaneSwitch is None or edge.getID() not in HttpInterface.emergencyLaneSwitch:
    #     return '未查询到对应应急车道'
    # if HttpInterface.emergencyLaneSwitch[edge.getID()] == action:
    #     return '未改变状态'
    lane_id = edge.getID() + '_0'
    if edge.getFromNode().getID() in tl_ids:
        state = traci.trafficlight.getRedYellowGreenState(edge.getFromNode().getID())
    if action == OPEN:
        # print("开启应急车道")
        aa = edge.getFromNode().getID()
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), 'G' + state[1:])
        traci.lane.setDisallowed(lane_id, [])
        # HttpInterface.emergencyLaneSwitch[edge.getID()] = action
    elif action == DOWN or action == -1:
        # print("关闭应急车道")
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), 'r' + state[1:])
        traci.lane.setAllowed(lane_id, EMERGENCY_LANE_ALLOW_VCLASS)
        # HttpInterface.emergencyLaneSwitch[edge.getID()] = action
    else:
        # print("删除应急车道")
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), 'G' + state[1:])
        traci.lane.setDisallowed(lane_id, [])
        # del HttpInterface.emergencyLaneSwitch[edge.getFrom().getID()]


# 初始化专用车道
def initExclusiveLaneConfig(start_edge, end_edge, tlss):
    # 1.计算edge集合和信号灯集合
    edge_list, traffic_list = findEdgesAndTraffics(start_edge, end_edge, tlss, stop_edge=STOP_EDGE)

    # 2.校验并添加专用车道
    for edge_id in edge_list:
        # 2.1.不可处于连接点
        assert ':' not in edge_id, '交叉路口无法设置专用车道'
        # 2.2.车道数量>2
        lane_number = traci.edge.getLaneNumber(edge_id)
        assert lane_number > 2, '道路数量小于3时无法设置专用车道'
        # 2.3.最左侧车道宽度>=3m
        lane_id = str(edge_id) + '_' + str(lane_number - 1)
        lane_width = traci.lane.getWidth(lane_id)
        assert lane_width >= 3, '路宽小于3m时无法设置专用车道'
        # 2.4.只允许编队车辆通行
        traci.lane.setAllowed(lane_id, EXCLUSIVE_LANE_ALLOW_VCLASS)
        # 2.6.更新exclusiveLaneSwitch属性
        HttpInterface.exclusiveLaneSwitch[edge_id] = DOWN

    # 3.增加应急车道指示灯
    for traffic_id in traffic_list:
        state = traci.trafficlight.getRedYellowGreenState(traffic_id)
        # 应急车道在最左侧 所以替换最后一个灯色
        state = state[:-1] + 'r'
        traci.trafficlight.setRedYellowGreenState(traffic_id, state)
    message = "应急车道添加成功"
    return message


def controlExclusiveLane(action: int, edge, tl_ids):
    # 匹配道路
    if HttpInterface.exclusiveLaneSwitch is None or edge.getID() not in HttpInterface.exclusiveLaneSwitch:
        return '未查询到对应专用车道'
    # if HttpInterface.exclusiveLaneSwitch[edge.getID()] == action:
    #     return '未改变状态'
    lane_id = edge.getID() + '_' + str(edge.getLaneNumber() - 1)
    if edge.getFromNode().getID() in tl_ids:
        state = traci.trafficlight.getRedYellowGreenState(edge.getFromNode().getID())
    if action == OPEN:
        # print("开启专用车道")
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), state[:-1] + 'G')
        traci.lane.setDisallowed(lane_id, [])
        HttpInterface.exclusiveLaneSwitch[edge.getID()] = action
    elif action == DOWN:
        # print("关闭专用车道")
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), state[:-1] + 'r')
        traci.lane.setAllowed(lane_id, EXCLUSIVE_LANE_ALLOW_VCLASS)
        HttpInterface.exclusiveLaneSwitch[edge.getID()] = action
    else:
        # print("删除专用车道")
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), state[:-1] + 'G')
        traci.lane.setDisallowed(lane_id, [])
        del HttpInterface.exclusiveLaneSwitch[edge.getFrom().getID()]


def bad_road_change_lane(plexe, bad_road: str, leave_road: str, bad_change_veh_set: set):
    """
    大于三车道坏路变道（左拐）
    :param plexe: 变道模型类
    :param bad_road: 坏路junction标记
    :param leave_road: 离开坏路的junction标记
    :param change_veh_records: 被set_fixed_lane的车辆集合，离开时disable_fixed_lane
    :return:
    """
    bad_result = traci.junction.getContextSubscriptionResults(bad_road)
    if len(bad_result) > 0:
        for veh_id, veh_info in dict(bad_result).items():
            if veh_info[tc.VAR_TYPE] != 'vtypeauto' and veh_info[tc.VAR_LANE_INDEX] < 2:
                print("左拐")
                left_followers = traci.vehicle.getLeftFollowers(veh_id)
                if len(left_followers) > 0:
                    print("左后方有车")
                    if abs(left_followers[0][1]) < 7:
                        if left_followers[0][0] not in bad_change_veh_set:
                            plexe.set_fixed_lane(left_followers[0][0], int(veh_info[tc.VAR_LANE_INDEX]) + 2,
                                                 False)
                            bad_change_veh_set.add(left_followers[0][0])
                    else:
                        plexe.set_fixed_lane(veh_id, int(veh_info[tc.VAR_LANE_INDEX]) + 1, False)
                        bad_change_veh_set.add(veh_id)
                else:
                    plexe.set_fixed_lane(veh_id, int(veh_info[tc.VAR_LANE_INDEX]) + 1, False)
                    bad_change_veh_set.add(veh_id)

    leave_result = traci.junction.getContextSubscriptionResults(leave_road)
    if len(leave_result) > 0:
        for veh_id in list(bad_change_veh_set):
            if veh_id in leave_result:
                plexe.disable_fixed_lane(veh_id)
                bad_change_veh_set.remove(veh_id)


def findNewEdge(oldEdge):
    if INTERNAL_SIGN not in str(oldEdge):
        return oldEdge
    for k, _ in oldEdge.getOutgoing().items():
        return k


"""
 获取中心坐标
 :param geolocations: [(lat, lon)]
"""


def center_geolocation(geolocations: list):
    x = 0
    y = 0
    z = 0
    length = len(geolocations)
    for lat, lon in geolocations:
        lon = radians(float(lon))
        lat = radians(float(lat))
        x += cos(lat) * cos(lon)
        y += cos(lat) * sin(lon)
        z += sin(lat)
    x = float(x / length)
    y = float(y / length)
    z = float(z / length)
    return (degrees(atan2(y, x)), degrees(atan2(z, sqrt(x * x + y * y))))


def get_time_to_reach_target(current_speed: str, current_acceleration: str, target_speed: str,
                             distance_to_target: str) -> float:
    # 将输入字符串转换为浮点数
    current_speed = float(current_speed)
    current_acceleration = float(current_acceleration)
    target_speed = float(target_speed)
    distance_to_target = float(distance_to_target)

    # 计算到达指定位置所需的时间
    time = (target_speed - current_speed) / current_acceleration + math.sqrt(
        2 * distance_to_target / current_acceleration + (target_speed - current_speed) ** 2 / current_acceleration ** 2)
    return time


def calc_time_to_reach2(target_speed, target_distance, current_speed, current_acceleration):
    # 将字符串类型的参数转换为浮点数类型
    v = float(target_speed)
    d = float(target_distance)
    v0 = float(current_speed)
    a = float(current_acceleration)
    t = (v - v0) / a + (d - (v ** 2 - v0 ** 2) / (2 * a)) / v
    return t


def calc_time_to_reach(target_speed, target_distance, current_speed, current_acceleration):
    # 将字符串类型的参数转换为浮点数类型
    target_speed = float(target_speed)
    target_distance = float(target_distance)
    current_speed = float(current_speed)
    current_acceleration = float(current_acceleration)

    # 如果加速度为负数，则表示减速
    if current_acceleration < 0:
        current_acceleration = abs(current_acceleration)

    # 计算加速度对应的时间和距离
    t_accel = (target_speed - current_speed) / current_acceleration
    d_accel = current_speed * t_accel + 0.5 * current_acceleration * t_accel ** 2

    # 如果需要的距离小于加速阶段行驶的距离，则只需考虑加速阶段的时间
    if target_distance <= d_accel:
        t_total = 2 * math.sqrt(target_distance / current_acceleration) + (
                target_speed - current_speed) / current_acceleration
    else:
        # 否则需要先加速到目标速度，再匀速行驶，最后减速到目标速度
        t_accel_to_target = (target_speed - current_speed) / current_acceleration
        d_accel_to_target = current_speed * t_accel_to_target + 0.5 * current_acceleration * t_accel_to_target ** 2

        d_decel_to_target = target_distance - d_accel_to_target
        t_decel_to_target = math.sqrt(2 * d_decel_to_target / current_acceleration)

        t_total = t_accel_to_target + t_decel_to_target + (d_decel_to_target / target_speed)

    return t_total


DEFAULT_CONFIG_FILE_PATH = './webgl/长安大学_openDrive_v0.sumocfg'


def start_sumo(config_file=DEFAULT_CONFIG_FILE_PATH, already_running=False, custom_arg=None, gui=True):
    """
    Starts or restarts sumo with the given configuration file
    :param config_file: sumo configuration file
    :param already_running: if set to true then the command simply reloads
    the given config file, otherwise sumo is started from scratch
    :param custom_arg: label param
    :param gui: start GUI or not
    """
    # 检测是否已经添加环境变量
    if 'SUMO_HOME' in os.environ:
        tools = os.path.join(os.environ['SUMO_HOME'], 'tools')
        sys.path.append(tools)
    else:
        sys.exit("please declare environment variable 'SUMO_HOME'")
    if custom_arg is None:
        custom_arg = build_custom_arg(delay=900, config_file=config_file)
    sumo_cmd = [sumolib.checkBinary('sumo-gui' if gui else 'sumo')]
    # custom_arg.append(config_file)
    if already_running:
        traci.load(custom_arg)
    else:
        sumo_cmd.extend(custom_arg)
        traci.start(sumo_cmd)
        if gui:
            traci.gui.setSchema("View #0", "real world")


# xy转换经纬度
def xyConversionlatAndLon(sumo_net, pos):
    rule1 = sumo_net.getGeoProj()
    offset = sumo_net.getLocationOffset()

    longitude, latitude = rule1(pos[0] - offset[0], pos[1] - offset[1], inverse=True)
    return longitude, latitude


# 经纬度转换xy
def latAndLonConversionXY(sumo_net, pos):
    rule1 = sumo_net.getGeoProj()
    offset = sumo_net.getLocationOffset()

    x1, y1 = rule1(pos['lon'], pos['lat'])

    x = x1 + offset[0]
    y = y1 + offset[1]
    return x, y


def build_custom_arg(delay=1000, step_length=0.1, lane_change_duration=3,
                     config_file=DEFAULT_CONFIG_FILE_PATH):
    return ['--delay', str(delay),
            '--step-length', str(step_length),
            '--collision.check-junctions',
            '--lanechange.duration', str(lane_change_duration),
            '-c', config_file
            ]


# 匝道管控
# 统计最近一分钟通过匝道红绿灯车辆数量
vehicles = set()


def addRampControl(vehicleids, rampControlNum: int):
    global vehicles
    for vehicleid in vehicleids:
        try:
            lane_id = traci.vehicle.getLaneID(vehicleid)
            if "694" in lane_id:
                vehicles.add(vehicleid)
        except TraCIException as e:
            print(str(e))
    times = traci.simulation.getTime()

    if times % 60 == 0:
        if len(vehicles) > rampControlNum:
            traci.trafficlight.setRedYellowGreenState("gneJ0", "r")
            vehicles.clear()
        else:
            traci.trafficlight.setRedYellowGreenState("gneJ0", "G")


def getFreeRoute(sumo_net_file: str, routeid: str, vehiclePoints: list, result: list):
    if len(vehiclePoints) == 0:
        # vehicleNum=50
        vehiclePoints = '[]'
        # vehiclePoints='[{"lat":34.372449,"lon":108.892912},{"lat":34.375380,"lon":108.899382}]'
        result = [{"arrive": [],
                   "depart": {"autonomousRatio": 0, "busRatio": 0, "count": 0, "point": [], "simplePassengerRatio": 0},
                   "id": "",
                   "route": [],
                   "type": 1}
                  ]
    sumo_net = sumolib.net.readNet(sumo_net_file, withInternal=True, withLatestPrograms=True)
    # vehiclePointsobj = json.loads(vehiclePoints)
    # for vehiclePoint in vehiclePoints:
    # lat = float(vehiclePoint["lat"])
    lat = vehiclePoints[1]
    # lon = float(vehiclePoint["lon"])
    lon = vehiclePoints[0]
    x, y = sumo_net.convertLonLat2XY(lon, lat)
    sumo_cfg_file = "D:\pythonproject\changansumo\CADXWCFZ_sumo\webgl\长安大学_openDrive_v0.sumocfg"
    # start_sumo(sumo_cfg_file, False, gui=False)
    traci.start(cmd=['sumo',
                     '-c', sumo_cfg_file,
                     '--step-length', str(0.1),
                     # '--lateral-resolution', '0.25',
                     '--collision.check-junctions',
                     # '--fcd-output', out_file,
                     # "--fcd-output.geo",
                     '--lanechange.duration', '1']
                )
    # x,y=sumo_utils.latAndLonConversionXY(sumo_net,(lon, lat))
    startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(lon, lat, True)
    traci.close()
    # startedgeID='-291.0.00'
    single_edge = sumo_net.getEdge(startedgeID)
    if single_edge is not None:
        route = [startedgeID]
        count = 0
        while single_edge is not None:
            edge_Bound = single_edge.getOutgoing()
            edgeids = list()
            for k, v in edge_Bound.items():
                edgeids.append(k.getID())
            if edgeids is not None and len(edgeids) > 0:
                try:
                    id = random.choice(edgeids)
                    if True:
                        route.append(id)
                    else:
                        logging.info("Road marking problem1")
                        errorMsg = "标注错误，请重新标注"
                    single_edge = sumo_net.getEdge(id)
                except ValueError:
                    continue
                # break
            count += 1
            if len(route) > 1000 or count > 200 or '-547.0.00' in route:
                print(route)
                break
        # routeid = str(uuid.uuid1())
        routes = list()
        for edge_id in route:
            if len(str(edge_id).strip()) == 0:
                continue
            edge = sumo_net.getEdge(edge_id)
            if edge_id == '-301.0.00' or edge_id == '-302.0.00' or edge_id == '-276.0.00' or edge_id == '-266.0.00' or edge_id == '-268.0.00' or edge_id == '-260.0.00' or edge_id == '-264.0.00' or edge_id == '-560.0.00' or edge_id == '-549.0.00':
                shape = edge.getRawShape3D()
                for sh in shape:
                    point = sumo_net.convertXY2LonLat(sh[0], sh[1])
                    routes.append(point)
            else:
                shape = edge.getRawShape3D()
                start_coord = shape[0]  # x, y, z of the start of the edge
                center_coord = shape[len(shape) // 2]  # x, y, z of the center of the edge
                end_coord = shape[-1]  # x, y, z of the end of the edge
                # point = sumo_net.convertXY2LonLat(shape[0][0], shape[0][1])
                start_coord_point = sumo_net.convertXY2LonLat(start_coord[0], start_coord[1])
                center_coord_point = sumo_net.convertXY2LonLat(center_coord[0], center_coord[1])
                end_coord_point = sumo_net.convertXY2LonLat(end_coord[0], end_coord[1])
                routes.append(start_coord_point)
                routes.append(center_coord_point)
                routes.append(end_coord_point)
            # if len(routes) > 0:

        result[0]['route'] = routes
        result[0]['id'] = routeid
        # result.append(RouteDetail(routeid, routes, v_count=0, a_type=2, simple_passenger_ratio=0.0, bus_ratio=0.0,
        #                 autonomous_ratio=0.0).to_dict())

        # traci.route.add(routeid, route)




    else:
        logging.info("Road marking problem1")
        errorMsg = "标注错误，请重新标注"
    return result


# # 获取指定车周围车辆
# def get_surrounding_vehicles(vehicle_id, radius):
#     surrounding_vehicles = []
#     x, y = traci.vehicle.getPosition(vehicle_id)
#     for v in traci.vehicle.getIDList():
#         if v == vehicle_id:
#             # 排除要查询的车辆本身
#             continue
#         v_x, v_y = traci.vehicle.getPosition(v)
#         distance = ((v_x - x)**2 + (v_y - y)**2)**0.5
#         if distance <= radius:
#             surrounding_vehicles.append(v)
#     return surrounding_vehicles
# 获取指定车周围车辆
def get_surrounding_vehicles(tag_vehicle_id, global_vehicles, radius):
    surrounding_vehicles = []
    x, y = traci.vehicle.getPosition(tag_vehicle_id)
    for vehicle_id, veh_info in dict(global_vehicles).items():
        if vehicle_id == vehicle_id:
            # 排除要查询的车辆本身
            continue
        v_x, v_y = veh_info[tc.VAR_POSITION]
        distance = ((v_x - x) ** 2 + (v_y - y) ** 2) ** 0.5
        if distance <= radius:
            surrounding_vehicles.append(vehicle_id)
    return surrounding_vehicles

# import traci
# traci.vehicle.couldChangeLane()
# traci.lane.getLinks()

can_change_lane = []
def background_vehicle_chang_lane(global_vehicles,vid, traci):
    try:
        # 得到真实车辆前小于100米的内的车辆
        leader_id,leader_dis = traci.vehicle.getLeader(vid)
        if leader_id is not None and leader_id in sumo_main.depart_info.keys():
            if leader_dis < 100:
                CV_lane_index = traci.vehicle.getLaneIndex(vid)
                leader_lane_index = traci.vehicle.getLaneIndex(leader_id)
                # print("领头车",leader_id,"所在车道为",leader_lane_index)
                if CV_lane_index == leader_lane_index:
                    MTF_change_lane(traci, leader_id, leader_lane_index, global_vehicles)
                    # print("避让----------")
    except Exception as e:
        logging.info("车辆避让异常" + str(e))


def MTF_change_lane(traci,leader_id,leader_lane_index,global_vehicles):
    global can_change_lane
    couldChangeLaneFlagRigh = traci.vehicle.couldChangeLane(leader_id, 1)
    couldChangeLaneFlagLeft = traci.vehicle.couldChangeLane(leader_id, -1)
    # print("是否可以右转", couldChangeLaneFlagRigh)
    # print("是否可以左转", couldChangeLaneFlagLeft)
    # 领头车车可以转向，直接转向
    if couldChangeLaneFlagRigh and not couldChangeLaneFlagLeft:
        traci.vehicle.changeLaneRelative(leader_id, 1, 1)
        can_change_lane.append(leader_id)
    elif couldChangeLaneFlagLeft and not couldChangeLaneFlagRigh:
        traci.vehicle.changeLaneRelative(leader_id, -1, 1)
        can_change_lane.append(leader_id)
    elif couldChangeLaneFlagLeft and couldChangeLaneFlagRigh:
        traci.vehicle.changeLaneRelative(leader_id, -1, 1)
        can_change_lane.append(leader_id)
    # 领头车不可以转向，说明领头车两侧车道均存在车辆，因此需要将领头车两侧车道的某一侧阻碍领头车变道的车辆进行变道
    else:
        if leader_id not in can_change_lane:
            # 领头车在第0、1车道，则向1、2车道变道
            # 领头车在第2、3车道，则向1、2车道变道
            current_lane_index = leader_lane_index
            if current_lane_index == 0 or current_lane_index == 1 :
                change_next_next_lane(traci,global_vehicles,leader_id,current_lane_index,1)
            if current_lane_index == 2 or current_lane_index == 3:
                change_next_next_lane(traci,global_vehicles,leader_id,current_lane_index,-1)
def change_next_next_lane(traci,global_vehicles,leader_id,current_lane_index,flag):
    global can_change_lane
    leader_pos = global_vehicles[leader_id][tc.VAR_POSITION]
    min_distance = float('inf')
    nearest_vehicle_id = None
    for vehicle_id, veh_info in dict(global_vehicles).items():
        lane_index = veh_info[tc.VAR_LANE_INDEX]

        if lane_index == current_lane_index + flag:
            pos = veh_info[tc.VAR_POSITION]
            distance = math.sqrt((leader_pos[0] - pos[0]) ** 2 + (
                    leader_pos[1] - pos[1]) ** 2)
            if distance < min_distance:
                min_distance = distance
                nearest_vehicle_id = vehicle_id
    # print("距离最近的车辆信息为", nearest_vehicle_id)
    if nearest_vehicle_id is not None:
        traci.vehicle.setLateralLanePosition(nearest_vehicle_id, flag, laneID=traci.vehicle.getLaneID(nearest_vehicle_id))
        traci.vehicle.changeLaneRelative(nearest_vehicle_id, flag, 1)
    traci.vehicle.changeLaneRelative(leader_id, flag, 1)
    traci.vehicle.setLateralLanePosition(leader_id,flag,laneID=traci.vehicle.getLaneID(leader_id))
